#pragma once
#include <libbase/common.h>
#include <rclcpp/rclcpp.hpp>
#include <base_interfaces/msg/armors.hpp>
#include <eigen3/Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include "tf2/exceptions.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

using std::placeholders::_1;

namespace wmj
{
    /**
     * @brief 单目测距类，继承自rclcpp::Node
     */
    class Monocular : public rclcpp::Node
    {
    private:
        double m_small_width;                               ///< 小装甲板宽度
        double m_small_height;                              ///< 小装甲板高度
        double m_large_width;                               ///< 大装甲板宽度
        double m_large_height;                              ///< 大装甲板高度
        std::vector<cv::Point3f> m_obj_large, m_obj_small;  ///< 大小装甲板的世界系坐标       
        std::vector<std::string> m_camera_frame_id;         ///< 相机坐标系
        std::vector<cv::Mat> m_CameraMat;                   ///< 相机内参矩阵
        std::vector<cv::Mat> m_DistMat;                     ///< 相机畸变矩阵
        int m_debug;
    public:
        /**
         * @brief 构造函数
         * @param name 节点名称
         */
        Monocular(std::string name);
        ~Monocular(){};
        
    private:
        /**
         * @brief 设置参数
         * @param file_path 文件路径
         */
        void setParam(const std::string &file_path);
        /**
         * @brief 获取位置和角度
         * @param vertices 装甲板角点，左下角为0，左上角为1，右上角为2，右下角为3
         * @param armor_type 装甲板类型
         * @param frame_id 相机坐标系
         * @param pose 装甲板在相机系下位姿
         * @param yaw_angle 无用的yaw_angle
         * @return 是否成功获取位姿和角度
         */
        bool getPositionAngle(std::vector<cv::Point2f> vertices, wmj::ARMORTYPE armor_type, std::string frame_id, geometry_msgs::msg::Pose &pose, double &yaw_angle);
        /**
         * @brief 装甲板回调函数
         * @param msg 一个相机一帧图像的装甲板识别结果
         */
        void armor_cb(const base_interfaces::msg::Armors::SharedPtr msg);
        rclcpp::Publisher<base_interfaces::msg::Armors>::SharedPtr m_armors_pub;
        rclcpp::Subscription<base_interfaces::msg::Armors>::SharedPtr m_armors_sub;

    private:
        /**
         * @brief 获取装甲板世界坐标系的坐标点
         * @param _width 宽度
         * @param _height 高度
         * @param armor_type 装甲板类型
         */
        void getObjPoints(float _width, float _height, wmj::ARMORTYPE armor_type);
    };
}